function state = estimate_mcl_state(particles)
% 从粒子估计状态
% 输入: particles-粒子数组
% 输出: state-估计状态

    N = length(particles);
    weights = [particles.weight];
    
    x = 0;
    y = 0;
    sin_sum = 0;
    cos_sum = 0;
    
    for i = 1:N
        x = x + weights(i) * particles(i).pose(1);
        y = y + weights(i) * particles(i).pose(2);
        sin_sum = sin_sum + weights(i) * sin(particles(i).pose(3));
        cos_sum = cos_sum + weights(i) * cos(particles(i).pose(3));
    end
    
    theta = atan2(sin_sum, cos_sum);
    state = [x; y; theta];
end

